
#include <stdio.h>
#include <BSphere>
#include "IAlgorithm.h"
#include "member_BShape.h"
#include "member_BBody.h"
#include "member_BNode.h"
#include "IGjkConvexCast.h"
#include "IContinuousConvexCast.h"
#include "IContinuousConvexPlaneCast.h"

using namespace BWE;

IAlgorithm::IAlgorithm()
{
	_bodyA = 0;
	_shapeA = 0;
	_bodyB = 0;
	_shapeB = 0;
	numContact = 0;
}
IAlgorithm::~IAlgorithm()
{

}

void IAlgorithm::swap()
{
	bSwap(_bodyA, _bodyB);
	bSwap(_shapeA, _shapeB);
	bSwap(_matrixA, _matrixB);
}

void IAlgorithm::setBodyA(BBody* body, BShape* shape, const BMatrix& matrix)
{
	_bodyA = body;
	_shapeA = shape;
	_matrixA = matrix;
}
void IAlgorithm::setBodyB(BBody* body, BShape* shape, const BMatrix& matrix)
{
	_bodyB = body;
	_shapeB = shape;
	_matrixB = matrix;
}

void IAlgorithm::timeImpact()
{
	BReal resultFraction = 1;

	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	if (BGridPlane* gridplane = dynamic_cast<BGridPlane*>(_shapeA))
	{
		CastResult caseResult;
		IContinuousConvexPlaneCast ccc(_shapeB, gridplane);
		ccc.calcTimeOfImpact(matrixB, interpMatrixB, matrixA, interpMatrixA, caseResult);
		return;
	}
	if (BGridPlane* gridplane = dynamic_cast<BGridPlane*>(_shapeB))
	{
		CastResult caseResult;
		IContinuousConvexPlaneCast ccc(_shapeA, gridplane);
		ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
		return;
	}
	if (0)
	{
		CastResult caseResult;
		IContinuousConvexCast ccc(_shapeA, _shapeB);
		ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
		return;
	}

	BReal squareMot0 = (interpMatrixA.position() - matrixA.position()).length2();
	BReal squareMot1 = (interpMatrixB.position() - matrixB.position()).length2();
	printf("squareMot : %lf, %lf.\n", squareMot0, squareMot1);

	const BBox& boxA = _bodyA->box();
	const BBox& boxB = _bodyB->box();

	{
		BSphere sphereB(boxB.radius());
		CastResult caseResult;
		IVoronoiSimplexSolver voronoiSimplex;
		IGjkConvexCast ccd(_shapeA, &sphereB, &voronoiSimplex);
		if (ccd.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult))
		{
			if (resultFraction > caseResult.fraction)
				resultFraction = caseResult.fraction;
		}
	}

	{
		BSphere sphereA(boxA.radius());
		CastResult caseResult;
		IVoronoiSimplexSolver voronoiSimplex;
		IGjkConvexCast ccd(&sphereA, _shapeB, &voronoiSimplex);
		if (ccd.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult))
		{
			if (resultFraction > caseResult.fraction)
				resultFraction = caseResult.fraction;
		}
	}
}

void IAlgorithm::addContactPoint(const BVector& pointB, const BVector& normalB, BReal distance, BReal fraction)
{
	BVector pointA = pointB + normalB * distance;
	BVector localPointA = _matrixA.inverse() * pointA;
	BVector localPointB = _matrixB.inverse() * pointB;

	BContact contact;
	contact.fraction = fraction;
	contact.distance = distance;
	contact.localPointA = localPointA;
	contact.localPointB = localPointB;
	contact.normalA = -normalB;
	contact.normalB = normalB;
	contact.pointA = pointA;
	contact.pointB = pointB;

	contact.restitution = body_member(_bodyA)->restitution * body_member(_bodyB)->restitution;
	contact.friction = body_member(_bodyA)->friction * body_member(_bodyB)->friction;

	BReal rollingFrictionA = body_member(_bodyA)->rollingFriction * body_member(_bodyB)->friction;
	BReal rollingFrictionB = body_member(_bodyB)->rollingFriction * body_member(_bodyA)->friction;
	contact.rollingFriction = bClamp(rollingFrictionA + rollingFrictionB, -10, 10);

	PlaneSpace(contact.normalB, contact.rollingFrictionDir1, contact.rollingFrictionDir2);
	contact.rollingFrictionDir1.normalize();
	contact.rollingFrictionDir2.normalize();

	if (numContact == 4)
	{
		int index = this->sortContacts(contact);
		contacts[index] = contact;
	}
	else
	{
		int index = numContact++;
		contacts[index] = contact;
	}
}
int IAlgorithm::sortContacts(const BContact& contact)
{
	int maxPenetrationIndex = -1;
	BReal maxPenetration = contact.distance;
	for (int i = 0; i < 4; i++)
	{
		if (contacts[i].distance < maxPenetration)
		{
			maxPenetrationIndex = i;
			maxPenetration = contacts[i].distance;
		}
	}
	BReal res0(0), res1(0), res2(0), res3(0);
	if (maxPenetrationIndex != 0)
	{
		BVector a0 = contact.localPointA - contacts[1].localPointA;
		BVector b0 = contacts[3].localPointA - contacts[2].localPointA;
		BVector cross = a0.cross(b0);
		res0 = cross.length2();
	}
	if (maxPenetrationIndex != 1)
	{
		BVector a1 = contact.localPointA - contacts[0].localPointA;
		BVector b1 = contacts[3].localPointA - contacts[2].localPointA;
		BVector cross = a1.cross(b1);
		res1 = cross.length2();
	}
	if (maxPenetrationIndex != 2)
	{
		BVector a2 = contact.localPointA - contacts[0].localPointA;
		BVector b2 = contacts[3].localPointA - contacts[1].localPointA;
		BVector cross = a2.cross(b2);
		res2 = cross.length2();
	}
	if (maxPenetrationIndex != 3)
	{
		BVector a3 = contact.localPointA - contacts[0].localPointA;
		BVector b3 = contacts[2].localPointA - contacts[1].localPointA;
		BVector cross = a3.cross(b3);
		res3 = cross.length2();
	}
	res0 = fabs(res0);
	res1 = fabs(res1);
	res2 = fabs(res2);
	res3 = fabs(res3);
	int maxIndex = -1;
	BReal maxVal = -REAL_LARGE;
	if (res0 > maxVal)
	{
		maxIndex = 0;
		maxVal = res0;
	}
	if (res1 > maxVal)
	{
		maxIndex = 1;
		maxVal = res1;
	}
	if (res2 > maxVal)
	{
		maxIndex = 2;
		maxVal = res2;
	}
	if (res3 > maxVal)
	{
		maxIndex = 3;
	}
	return maxIndex;
}
void IAlgorithm::refreshContactPoints()
{
	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;
	for (int i = 0; i < numContact; i++)
	{
		BContact& contact = contacts[i];
		contact.distance = (contact.pointA - contact.pointB).dot(contact.normalB);
	}
	for (int i = numContact - 1; i >= 0; i--)
	{
		BContact& contact = contacts[i];
		bool removal = true;
		if (contact.distance < REAL_EPSILON)
		{
			BVector projectedPoint = contact.pointA - contact.normalB * contact.distance;
			BVector projectedDifference = contact.pointB - projectedPoint;
			BReal distance2 = projectedDifference.length2();
			if (distance2 < REAL_EPSILON)
			{
				removal = false;
			}
		}
		if (removal)
		{
			if (i < numContact - 1)
			{
				contacts[i] = contacts[numContact - 1];
			}
			numContact--;
		}
	}
}